#include <config.h>
#include <board.h>
#include <init.h>
#include <os.h>
#include "stm32f0xx.h"
#include "stm32f0xx_conf.h"
#include "device.h"
#include "printk.h"
#include "stm32_flash.h"

#include "settings.h"
#ifdef configUSING_IFLASH

#define PAGE_SIZE (1024u)

typedef struct flash_dev
{
    struct device                dev;
    struct device_blk_geometry   geometry;
    void                        *user_data;
} flash_dev_t;

static flash_dev_t iflash;

/*
 * lock and unlock
 */
static void flash_lock(flash_dev_t *flash)
{
		FLASH_Lock();
}
static void flash_unlock(flash_dev_t *flash)
{
	  FLASH_Unlock();
}

/*
 * erase flash
 */
//static u32 es8p508_erase(u32 page_addr)
//{
//		FLASH_ErasePage(page_addr);
//    return PAGE_SIZE;
//}
static uint32_t iflash_erase(device_t *dev, off_t pos, size_t size)
{
	  FLASH_Unlock();
    FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR);
    uint8_t page_num = (size + FLASH_SECTOR_SIZE - 1) / FLASH_SECTOR_SIZE;
    while (page_num--)
    {
        while (FLASH_COMPLETE != FLASH_ErasePage(pos))
            ;
        pos += FLASH_SECTOR_SIZE;
        IWDG_ReloadCounter();
    }
    FLASH_Lock();

    return size;
		
		
		
}

/*
 * read flash
 */
static size_t iflash_read(device_t *dev, off_t pos, void *buffer, size_t size)
{
	uint8_t * index = NULL;
	index = (uint8_t *)buffer;
	while (size--)
        *(index++) = *(__IO uint8_t *)(pos++);
    return size;
}

/*
 * write flash
 */
static int es8p508_flash_write(uint32_t addr, const void *data, uint32_t len)
{
    int ret = -1;

    if ((len == 0) || (len & 0x03) || ((uint32_t)data & 0x03))
	{
		log_d("flash_write error len: %d, data: %d\n", len, data);
        goto out;
	}

	int i;
	for (i = 0; i < len; i += 4)
    {
        if (FLASH_ProgramWord(addr, *((uint32_t *)data + i / 4)) == ERROR)
            goto out;
		addr += 4;
    }
    ret = len;
out:
    return ret;
}
static size_t iflash_write(device_t *dev, off_t pos, const void *data, size_t size)
{
	 uint32_t halfword_len = (size + 1) / 2;
    uint16_t halfword;
	  uint8_t * index = NULL;
	  index = (uint8_t *)data;
    FLASH_Unlock();
    FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR);
    while (halfword_len--)
    {
        halfword  = *(index++);
        halfword |= *(index++) << 8;
        while (FLASH_COMPLETE != FLASH_ProgramHalfWord(pos, halfword))
            ;
        pos += 2;
        IWDG_ReloadCounter();
    }
    FLASH_Lock();

    return size;
}

/*
 * control flash
 */
static err_t iflash_control(device_t *dev, uint8_t cmd, void *args)
{
    switch (cmd)
    {
    case DEVICE_CTRL_BLK_ERASE:
    {
        struct flash_erase_cmd *fec = (struct flash_erase_cmd *)args;

        iflash_erase(dev, fec->start, fec->size);
        break;
    }
    default:
        break;
    }

    return 0;
}

static const struct device_ops iflash_ops =
{
    .read    = iflash_read,
    .write   = iflash_write,
    .ctrl    = iflash_control,
};

//static void test_iflash(void) USED;
//static void test_iflash(void)
//{
//    u8 tmp[0x20];

//    memset(tmp, 0, sizeof(tmp));

//    iflash_erase(&iflash.dev, 64 * 1024, 1024);

//    iflash_read(&iflash.dev, 64 * 1024, tmp, sizeof(tmp));

//	int i;
//	for (i = 0; i < sizeof(tmp); i++)
//		tmp[i] = i;
//    iflash_write(&iflash.dev, 64 * 1024, tmp, sizeof(tmp));

//    memset(tmp, 0, sizeof(tmp));
//    iflash_read(&iflash.dev, 64 * 1024, tmp, sizeof(tmp));

////    assert();
//}
static int board_add_iflash(void) USED;
static int board_add_iflash(void)
{
//	test_iflash();

    iflash.dev.ops = &iflash_ops;
    device_register(&iflash.dev, "iflash", 0);
    return 0;
}
device_initcall(board_add_iflash);
#endif

